#ifndef VOXEL_VOTING_FILTER_HPP
#define VOXEL_VOTING_FILTER_HPP


#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <Eigen/Geometry>


namespace mult_lidar{

    //体素滤波投票过滤器
    class VoxelVotingFilter{

        public:

        //创建三维网格
        struct VoxelKey{
            int x, y, z;

            bool operator==(const VoxelKey &other) const {
                return x == other.x && y == other.y && z == other.z;
            }
        };

        //体素哈希函数
        struct VoxelHash{
            std::size_t operator()(const VoxelKey& k) const {
                return ((std::hash<int>()(k.x) ^ (std::hash<int>()(k.y) << 1)) >> 1)
                        ^ (std::hash<int>()(k.z) << 1);
            }
        };

        VoxelVotingFilter(float resolution = 0.05f, int init_score = 5, int min_score = 3);
        void processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud);
        void setParameters(float resolution, int init_score, int min_score );



        private:

        VoxelKey pointToVoxel(const pcl::PointXYZI& point) const;
        std::vector<VoxelKey> computeRayVoxels(const Eigen::Vector4f& origin,
                                              const pcl::PointXYZI& end) const;


        float resolution_;
        int init_score_;
        int min_score_;
        std::unordered_map<VoxelKey, int, VoxelHash> voxel_scores_;


    };

}

#endif